#include "stdafx.h"
#include "VectorHelper.h"
#include "Vectors.h"
#include "Math.h"

using namespace VectorHelper;

// a vector in the direction { 0, 1, 0 }
/*extern*/ Vector3 vector_3::up = Vector3(0.f, 1.f, 0.f);
// a vector in the direction { 0, -1, 0 }
/*extern*/ Vector3 vector_3::down = Vector3(0.f, -1.f, 0.f);
// a vector in the direction { -1, 0, 0 }
/*extern*/ Vector3 vector_3::left = Vector3(-1.f, 0.f, 0.f);
// a vector in the direction { 1, 0, 0 }
/*extern*/ Vector3 vector_3::right = Vector3(1.f, 0.f, 0.f);
// a vector in the direction { 0, 0, -1 }
/*extern*/ Vector3 vector_3::forward = Vector3(0.f, 0.f, -1.f);
// a vector in the direction { 0, 0, 1 }
/*extern*/ Vector3 vector_3::backward = Vector3(0.f, 0.f, 1.f);

// a zero, 2-component vector
/*extern*/ Vector2 vector_2::Zero = Vector2(0.f, 0.f);


// a zero 3-component vector
extern Vector3 vector_3::Zero = Vector3(0.f);
// a zero 4-component vector
extern Vector4 vector_4::Zero = Vector4(0.f);

/*extern*/ Vector4 vector_4::red(1.f, 0.f, 0.f, 1.f);
/*extern*/ Vector4 vector_4::green(0.f, 1.f, 0.f, 1.f);
/*extern*/ Vector4 vector_4::blue(0.f, 0.f, 1.f, 1.f);
/*extern*/ Vector4 vector_4::yellow(1.f, 1.f, 0.f, 1.f);
/*extern*/ Vector4 vector_4::fuckUgly_magenta(1.f, 0.f, 1.f, 1.f);
/*extern*/ Vector4 vector_4::black(0.f, 0.f, 0.f, 1.f);
/*extern*/ Vector4 vector_4::white(1.f, 1.f, 1.f, 1.f);
/*extern*/ Vector4 vector_4::transparent(0.f, 0.f, 0.f, 0.f);

/*extern*/ Vector4 vector_4::brown(0.8f, 0.5f, 0.1f, 1.f);
/*extern*/ Vector4 vector_4::gray(.5f, .5f, .5f, 1.f);

extern Matrix Matricies::Identity = Matrix(1.f, 0.f, 0.f, 0.f,
										   0.f, 1.f, 0.f, 0.f,
										   0.f, 0.f, 1.f, 0.f,
										   0.f, 0.f, 0.f, 1.f);

Matrix Matricies::CreateTranslation(const Vector3& v)
{
	Matrix outval = Identity;
	outval.m41 = v.x;
	outval.m42 = v.y;
	outval.m43 = v.z;
	outval.m44 = 1.f;

	return outval;
}

Matrix Matricies::CreateRotationX(float radians)
{
	return Matrix(1.f, 0.f, 0.f, 0.f,
		0.f, cos(radians), -sin(radians), 0.f,
		0.f, sin(radians), cos(radians), 0.f,
		0.f, 0.f, 0.f, 1.f);
}

Matrix Matricies::CreateRotationY(float radians)
{
	return Matrix(cos(radians), 0.f, sin(radians), 0.f,
		0.f, 1.f, 0.f, 0.f,
		-sin(radians), 0.f, cos(radians), 0.f,
		0.f, 0.f, 0.f, 1.f);
}

Matrix Matricies::CreateRotationZ(float radians)
{
	return Matrix(cos(radians), -sin(radians), 0.f, 0.f,
		sin(radians), cos(radians), 0.f, 0.f,
		0.f, 0.f, 1.f, 0.f,
		0.f, 0.f, 0.f, 1.f);
}

Matrix Matricies::CreateScale(float scale)
{
	return Matrix(scale, 0.f, 0.f, 0.f,
		0.f, scale, 0.f, 0.f,
		0.f, 0.f, scale, 0.f,
		0.f, 0.f, 0.f, 1.f);
}

Matrix Matricies::CreateScale(float scale_x, float scale_y, float scale_z)
{
	return Matrix(scale_x, 0.f, 0.f, 0.f,
		0.f, scale_y, 0.f, 0.f,
		0.f, 0.f, scale_z, 0.f,
		0.f, 0.f, 0.f, 1.f);
}

// create a camera look at matrix
Matrix Matricies::CreateLookAt(Vector3& cameraPosition, Vector3& cameraLookAt, Vector3& cameraUp)
{
	Vector3 vector3_1 = cameraPosition - cameraLookAt;
	vector3_1.Normalize();
	Vector3 vector3_2 = cameraUp ^ vector3_1;
	vector3_2.Normalize();
	Vector3 vector1 = vector3_1 ^ vector3_2;
	Matrix matrix;
	matrix.m11 = vector3_2.x;
	matrix.m12 = vector1.x;
	matrix.m13 = vector3_1.x;
	matrix.m14 = 0.f;
	matrix.m21 = vector3_2.y;
	matrix.m22 = vector1.y;
	matrix.m23 = vector3_1.y;
	matrix.m24 = 0.f;
	matrix.m31 = vector3_2.z;
	matrix.m32 = vector1.z;
	matrix.m33 = vector3_1.z;
	matrix.m34 = 0.f;
	matrix.m41 = -(vector3_2 * cameraPosition);
	matrix.m42 = -(vector1 * cameraPosition);
	matrix.m43 = -(vector3_1 * cameraPosition);
	matrix.m44 = 1.f;
	return matrix;
}

// create a perspective matrix with field of view
Matrix Matricies::CreatePerspectiveFieldOfView(float fov, float aspectRatio, float nearPlaneDistance, float farPlaneDistance)
{
	// ensure field of view is between 0 and pi
	assert(fov >= 0.f && fov <= Math::PI && "Field of view must be between 0 and Pi");
	// ensure near plane is not less than zero
	assert(nearPlaneDistance >= 0.f && "Near plane distance cannot be lest than zero");
	// ensure far plane distance is not less than zero
	assert(farPlaneDistance >= 0.f && "Far plane distance cannot be less than zero");
	// ensure near plane distance is not greater than far plane distance
	assert(nearPlaneDistance < farPlaneDistance && "Near plane distance must be less than far plane distance");

	float num1 = 1.f / (float)tan((double)fov * double(.5));
	float num2 = num1 / aspectRatio;
	Matrix result;
	result.m11 = num2;
	result.m12 = result.m13 = result.m14 = 0.f;
	result.m22 = num1;
	result.m21 = result.m23 = result.m24 = 0.f;
	result.m31 = result.m32 = 0.f;
	result.m33 = farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
	result.m34 = -1.f;
	result.m41 = result.m42 = result.m44 = 0.f;
	result.m43 = (float)((double)nearPlaneDistance * (double)farPlaneDistance / ((double)nearPlaneDistance - (double)farPlaneDistance));

	return result;
}
Matrix Matricies::CreateFromAxisAngle(Vector3& axis, float angle)
{
	float n1 = axis.x;
	float n2 = axis.y;
	float n3 = axis.z;
	float n4 = cos(angle);
	float n5 = sin(angle);

	float n6 = n1 *  n1;
	float n7 = n2 * n2;
	float n8 = n3 * n3;
	float n9 = n1 * n2;
	float n10 = n1 * n3;
	float n11 = n2 * n3;

	Matrix outval;

	outval.m11 = n6 + n5 * (1.f - n6);
	outval.m12 = (float)((double)n9 - (double)n9 + (double)n4 * (double)n3);
	outval.m13 = (float)((double)n10 - (double)n5 * (double)n10 - (double)n4 * (double)n2);
	outval.m14 = 0.f;
	outval.m21 = (float)((double)n9 - (double)n5 * (double)n9 - (double)n4 * (double)n3);
	outval.m22 = n7 + n5 * (1.f - n7);
	outval.m23 = (float)((double)n11 - (double)n5 * (double)n11 + (double)n4 * (double)n1);
	outval.m24 = 0.f;
	outval.m31 = (float)((double)n10 - (double)n5 * (double)n10 + (double)n4 * (double)n2);
	outval.m32 = (float)((double)n11 - (double)n5 * (double)n11 - (double)n4 * (double)n1);
	outval.m33 = n8 + n5 * (1.f - n8);
	outval.m34 = 0.f;
	outval.m41 = 0.f;
	outval.m42 = 0.f;
	outval.m43 = 0.f;
	outval.m44 = 1.f;

	return outval;
}

Matrix Matricies::CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNear, float zFar)
{
	Matrix matrix;
	matrix.m11 = (float)(2.0 / ((double)right - (double)left));
	matrix.m12 = matrix.m13 = matrix.m14 = 0.0f;
	matrix.m22 = (float)(2.0 / ((double)top - (double)bottom));
	matrix.m21 = matrix.m23 = matrix.m24 = 0.0f;
	matrix.m33 = (float)(1.0 / ((double)zNear - (double)zFar));
	matrix.m31 = matrix.m32 = matrix.m34 = 0.0f;
	matrix.m41 = (float)(((double)left + (double)right) / ((double)left - (double)right));
	matrix.m42 = (float)(((double)top + (double)bottom) / ((double)bottom - (double)top));
	matrix.m43 = zNear / (zNear - zFar);
	matrix.m44 = 1.f;

	return matrix;
}

// RETURN:	The determinant of a 3x3 matrix
float Matrix_Determinant(float e_11, float e_12, float e_13,
	float e_21, float e_22, float e_23,
	float e_31, float e_32, float e_33)
{
	float i = e_11 * (e_22 * e_33 - e_32 * e_23);
	float j = e_12 * (e_21 * e_33 - e_31 * e_23);
	float k = e_13 * (e_21 * e_32 - e_31 * e_22);

	return i - j + k;
}

Matrix Matricies::Invert(Matrix& m)
{
	float det = Determinant(m);

	if (det == 0.f)
		return m;

#pragma region Top Row
	float m11 = Matrix_Determinant(m.m22, m.m23, m.m24,
		m.m32, m.m33, m.m34,
		m.m42, m.m43, m.m44);

	float m12 = -Matrix_Determinant(m.m21, m.m23, m.m24,
		m.m31, m.m33, m.m34,
		m.m41, m.m43, m.m44);

	float m13 = Matrix_Determinant(m.m21, m.m22, m.m24,
		m.m31, m.m32, m.m34,
		m.m41, m.m42, m.m44);

	float m14 = -Matrix_Determinant(m.m21, m.m22, m.m23,
		m.m31, m.m32, m.m33,
		m.m41, m.m42, m.m43);
#pragma endregion
#pragma region second Row
	float m21 = -Matrix_Determinant(m.m12, m.m13, m.m14,
		m.m32, m.m33, m.m34,
		m.m42, m.m43, m.m44);

	float m22 = Matrix_Determinant(m.m11, m.m13, m.m14,
		m.m31, m.m33, m.m34,
		m.m41, m.m43, m.m44);

	float m23 = -Matrix_Determinant(m.m11, m.m12, m.m14,
		m.m31, m.m32, m.m34,
		m.m41, m.m42, m.m44);

	float m24 = Matrix_Determinant(m.m11, m.m12, m.m13,
		m.m31, m.m32, m.m33,
		m.m41, m.m42, m.m43);
#pragma endregion
#pragma region third Row
	float m31 = Matrix_Determinant(m.m12, m.m13, m.m14,
		m.m22, m.m23, m.m24,
		m.m42, m.m43, m.m44);

	float m32 = -Matrix_Determinant(m.m11, m.m13, m.m14,
		m.m21, m.m23, m.m24,
		m.m41, m.m43, m.m44);

	float m33 = Matrix_Determinant(m.m11, m.m12, m.m14,
		m.m21, m.m22, m.m24,
		m.m41, m.m42, m.m44);

	float m34 = -Matrix_Determinant(m.m11, m.m12, m.m13,
		m.m21, m.m22, m.m23,
		m.m41, m.m42, m.m43);
#pragma endregion
#pragma region fourth Row
	float m41 = -Matrix_Determinant(m.m12, m.m13, m.m14,
		m.m22, m.m23, m.m24,
		m.m32, m.m33, m.m34);

	float m42 = Matrix_Determinant(m.m11, m.m13, m.m14,
		m.m21, m.m23, m.m24,
		m.m31, m.m33, m.m34);

	float m43 = -Matrix_Determinant(m.m11, m.m12, m.m14,
		m.m21, m.m22, m.m24,
		m.m31, m.m32, m.m34);

	float m44 = Matrix_Determinant(m.m11, m.m12, m.m13,
		m.m21, m.m22, m.m23,
		m.m31, m.m32, m.m33);
#pragma endregion


	Matrix c = { m11, m12, m13, m14,
		m21, m22, m23, m24,
		m31, m32, m33, m34,
		m41, m42, m43, m44 };

	for (int i = 0; i < 16; i++)
		c.e[i] /= det;

	return Transpose(c);

}

Matrix Matricies::Transpose(Matrix& m)
{
	float m11 = m.m11,
		m22 = m.m22,
		m33 = m.m33,
		m44 = m.m44,

		m41 = m.m14,
		m14 = m.m41,

		m13 = m.m31,
		m31 = m.m13,

		m12 = m.m21,
		m21 = m.m12,

		m42 = m.m24,
		m24 = m.m42,

		m32 = m.m23,
		m23 = m.m32,

		m43 = m.m34,
		m34 = m.m43;

	Matrix outval = { m11, m12, m13, m14,
		m21, m22, m23, m24,
		m31, m32, m33, m34,
		m41, m42, m43, m44, };

	return outval;
}


float Matricies::Determinant(Matrix& m)
{
	float _1 = m.m11 * Matrix_Determinant(m.m22, m.m23, m.m24,
		m.m32, m.m33, m.m34,
		m.m42, m.m43, m.m44);

	float _2 = m.m12 *  Matrix_Determinant(m.m21, m.m23, m.m24,
		m.m31, m.m33, m.m34,
		m.m41, m.m43, m.m44);

	float _3 = m.m13 * Matrix_Determinant(m.m21, m.m22, m.m24,
		m.m31, m.m32, m.m34,
		m.m41, m.m42, m.m44);

	float _4 = m.m14 * Matrix_Determinant(m.m21, m.m22, m.m23,
		m.m31, m.m32, m.m33,
		m.m41, m.m42, m.m43);

	return _1 - _2 + _3 - _4;
}


Vector4 vector_4::Lerp(Vector4& a, Vector4& b, float ammount)
{
	Vector4 outval;
	outval.setXyz(vector_3::Lerp(a.getXyz(), b.getXyz(), ammount));
	outval.w = Math::lerp(a.w, b.w, ammount);

	return outval;
}

Vector2 vector_2::Lerp(Vector2& a, Vector2& b, float ammount)
{
	Vector2 outval;
	outval.x = Math::lerp(a.x, b.x, ammount);
	outval.y = Math::lerp(a.y, b.y, ammount);

	return outval;
}

Vector3 vector_3::Lerp(Vector3& a, Vector3& b, float ammount)
{
	Vector3 outval;
	outval.setXy(vector_2::Lerp(a.getXy(), b.getXy(), ammount));
	outval.z = Math::lerp(a.z, b.z, ammount);

	return outval;
}

float VectorHelper::distance(Vector2& a, Vector2& b)
{
	return (a - b).getLength();
}

float VectorHelper::distance(Vector3& a, Vector3& b)
{
	return (a - b).getLength();
}
float VectorHelper::distance(Vector4& a, Vector4& b)
{
	return (a - b).getLength();
}

Quaternion Quaternoins::CreateFromAxisAngle(Vector3& axis, float angle)
{
	float half_angle = angle * .5f;
	float x = sin(half_angle);
	float y = cos(half_angle);
	Quaternion outval;
	outval.x = axis.x * x;
	outval.y = axis.y * x;
	outval.z = axis.z * x;
	outval.w  = y;

	return outval;
}

Quaternion Quaternoins::CreateFromYawPitchRoll(float yaw, float pitch, float roll)
{
	float num1 = roll * 0.5f;
	float num2 = sin((double)num1);
	float num3 = cos((double)num1);
	float num4 = pitch * 0.5f;
	float num5 = sin((double)num4);
	float num6 = cos((double)num4);
	float num7 = yaw * 0.5f;
	float num8 = sin((double)num7);
	float num9 = cos((double)num7);
	Quaternion result;
	result.x = (float)((double)num9 * (double)num5 * (double)num3 + (double)num8 * (double)num6 * (double)num2);
	result.y = (float)((double)num8 * (double)num6 * (double)num3 - (double)num9 * (double)num5 * (double)num2);
	result.z = (float)((double)num9 * (double)num6 * (double)num2 - (double)num8 * (double)num5 * (double)num3);
	result.w = (float)((double)num9 * (double)num6 * (double)num3 + (double)num8 * (double)num5 * (double)num2);
	return result;
}

Quaternion Quaternoins::CreateFromRotationMatrix(Matrix& matrix)
{
	float num1 = matrix.m11 + matrix.m22 + matrix.m33;
	Quaternion quaternion;
	if ((double)num1 > 0.0)
	{
		float num2 = sqrt((double)num1 + 1.0);
		quaternion.w = num2 * 0.5f;
		float num3 = 0.5f / num2;
		quaternion.x = (matrix.m23 - matrix.m32) * num3;
		quaternion.y  = (matrix.m31 - matrix.m13) * num3;
		quaternion.z = (matrix.m12 - matrix.m21) * num3;
	}
	else if ((double)matrix.m11 >= (double)matrix.m22 && (double)matrix.m11 >= (double)matrix.m33)
	{
		float num2 = sqrt(1.0 + (double)matrix.m11 - (double)matrix.m22 - (double)matrix.m33);
		float num3 = 0.5f / num2;
		quaternion.x = 0.5f * num2;
		quaternion.y = (matrix.m12 + matrix.m21) * num3;
		quaternion.z = (matrix.m13 + matrix.m31) * num3;
		quaternion.w = (matrix.m23 - matrix.m32) * num3;
	}
	else if ((double)matrix.m22 > (double)matrix.m33)
	{
		float num2 = sqrt(1.0 + (double)matrix.m22 - (double)matrix.m11 - (double)matrix.m33);
		float num3 = 0.5f / num2;
		quaternion.x = (matrix.m21 + matrix.m12) * num3;
		quaternion.y = 0.5f * num2;
		quaternion.z = (matrix.m32 + matrix.m23) * num3;
		quaternion.w = (matrix.m31 - matrix.m13) * num3;
	}
	else
	{
		float num2 = sqrt(1.0 + (double)matrix.m33 - (double)matrix.m11 - (double)matrix.m22);
		float num3 = 0.5f / num2;
		quaternion.x = (matrix.m31 + matrix.m13) * num3;
		quaternion.y = (matrix.m32 + matrix.m23) * num3;
		quaternion.z = 0.5f * num2;
		quaternion.w = (matrix.m12 - matrix.m21) * num3;
	}
	return quaternion;
}